#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
    力传感器信息采集
    2023/10/31
    maobinghao
"""
import rospy
import serial
import serial.tools.list_ports
import threading
import time
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Header

SERIAL_IS_OPEN = False  # 默认串口关闭
SERIAL_IS_GET = False  # 默认串口关闭
port_name_list = []  # 端口名称列表
port_com_list = []  # 端口号列表
MySerial = None  # 打开的串口

getFz = 0

hex_data1 = bytes([0xFE, 0x04, 0x00, 0x00, 0x00, 0x01, 0x25, 0xC5])
hex_data2 = bytes([0xFE, 0x04, 0x00, 0x01, 0x00, 0x01, 0x74, 0x05])
hex_data3 = bytes([0xFE, 0x04, 0x00, 0x02, 0x00, 0x01, 0x84, 0x05])
hex_datas = (hex_data1, hex_data2, hex_data3)

# 扫描可用的串口设备
def scan_serial():
    port_list = list(serial.tools.list_ports.comports())
    if len(port_list) > 0:
        global SERIAL_IS_GET
        SERIAL_IS_GET = True
        print(port_list)
        return port_list
    else:
        None_lis = []
        return None_lis

def send_data(ser, data):
    # 发送数据并计算CRC校验值
    ser.write(data)    

if __name__ == "__main__":
    rospy.init_node("force_three")
    serial_list = scan_serial()
    SERIAL_IS_OPEN = True
    rate = rospy.Rate(50)
    while SERIAL_IS_OPEN:
        # 扫描串口
        serial_list = scan_serial()
        if len(port_name_list) != len(serial_list):
            port_name_list.clear()
            port_com_list.clear()
            for i in range(len(serial_list)):
                port_name_list.append(serial_list[i].description)
                port_com_list.append(serial_list[i].device)

            print("Serial devices found:", port_com_list[0])
            SERIAL_IS_OPEN = False
        else:
            print("未扫描到串口设备")
    print("Serial devices found:", serial_list)
    # 打开串口
    ser = serial.Serial(port_com_list[0], 9600, timeout=0.1)
    ser.flushInput()
    ser.flushOutput()
    print("串口打开成功!!!")
    current_time = time.time()
    print("running")
    old_force_x = 0
    old_force_y = 0
    old_force_z = 0
    for k in range(20):
        pre_x = pre_y = pre_z = 0
        for i in range(3):
            if ser.isOpen():
                ser.write(hex_datas[i])
                time.sleep(0.01)
            else:
                raise ValueError('Failed to open serial port')
            data = ser.readline()
            # print("data\n",{data})
            if data:
                if i == 0:
                    pre_x = (data[3] << 8 | data[4])
                    if pre_x > 50000:
                        old_force_x += (pre_x - 65535)
                    else:
                        old_force_x += pre_x
                if i == 1:
                    pre_y = (data[3] << 8 | data[4])
                    if pre_y > 50000:
                        old_force_y += (pre_y - 65535)
                    else:
                        old_force_y += pre_y
                if i == 2:
                    pre_z = (data[3] << 8 | data[4])
                    if pre_z > 50000:
                        old_force_z += (pre_z - 65535)
                    else:
                        old_force_z += pre_z
        print(f"No. {k}: pre x {pre_x}, pre y {pre_y}, pre z {pre_z}")
    old_force_x = old_force_x / 20
    old_force_y = old_force_y / 20
    old_force_z = old_force_z / 20
    print("pre x   y   z", old_force_x, old_force_y, old_force_z)
    twist_force = TwistStamped()
    pub_sensor_readings = rospy.Publisher('/force_data', TwistStamped, queue_size=10)

    try:
        while True:
            try:
                if not ser:
                    ser = serial.Serial(port_com_list[0], 9600, timeout=0.1)
                    ser.flushInput()
                    ser.flushOutput()
                for i in range(3):
                    if ser.isOpen():
                        ser.write(hex_datas[i])
                        time.sleep(0.01)
                    else:
                        raise ValueError('Failed to open serial port')
                    data = ser.readline()
                    # print("data\n",{data})
                    if data:
                        if i == 0:
                            getFx = (data[3] << 8 | data[4])
                            if getFx > 50000:
                                getFx = getFx - 65535
                            getFx -= old_force_x
                            getFx = getFx / 100
                            twist_force.twist.linear.x = getFx
                        if i == 1:
                            getFy = (data[3] << 8 | data[4])
                            if getFy > 50000:
                                getFy = getFy - 65535
                            getFy -= old_force_y
                            getFy = getFy / 100
                            twist_force.twist.linear.y = getFy
                        if i == 2:
                            getFz = (data[3] << 8 | data[4])
                            if getFz > 50000:
                                getFz = getFz - 65535
                            getFz -= old_force_z
                            getFz = getFz / 100
                            twist_force.twist.linear.z = getFz
                        twist_force.header = Header(stamp=rospy.Time.now(), frame_id='base_link')
                        pub_sensor_readings.publish(twist_force)
            except serial.SerialException:
                print("采集数据出错")
            rate.sleep()
    except KeyboardInterrupt:
        print("程序已中断")
        if ser and ser.isOpen():
            ser.close()
        print("串口已关闭")
